1. Field of the Invention
The present invention relates to a dynamics simulation device, a dynamics simulation method, and a computer program for allowing a computer to reproduce a physical interaction of collision or contact between objects under a virtual reality environment or a telepresence environment in which plural objects coexist, and more particularly, to a dynamics simulation device, a dynamics simulation method, and a computer program for calculating a contact force or a collision force acting between rigid bodies contacting or colliding with each other in a rigid body dynamics simulation.
More specifically, the invention relates to a dynamics simulation device, a dynamics simulation method, and a computer program for calculating an action force f acting on a collision point by treating contact or collision events between the objects in an operational space for describing relations between forces acting on the objects and accelerations generated therefrom and solving motion equations including linear equations on the operational space in which a force f acting on a physical quantity x and an acceleration generated therefrom are correlated with each other using an operational space inertial inverse matrix Λ−1 and a linear complementary problem including linear inequality constraints applied to elements of the action forces f acting on collision points, and still more particularly, to a dynamics simulation device, a dynamics simulation method, and a computer program for calculating a vertical repulsive force fNi and a frictional force fFi acting on a collision point i by solving the linear complementary problem after calculating the operational space inertial inverse matrix Λ−1.
2. Description of the Related Art
In the technical fields of games or virtual reality, there is a need for improvement in reality and interactivity of motion information based on physical laws using dynamics simulations in addition to visual information and audio information. In such a type of dynamics simulations, it is necessary to detect that a collision between cubes occurs and to apply a proper repulsive force on a collision point of the colliding objects.
As a method of detecting the collision between the objects, for example, in a situation where cubes contact or collide with each other, a contact shape calculating method of calculating a contact polygonal formed therebetween completely and at a high speed has been suggested (for example, see JP-A-2007-102394). In the contact shape calculating method, a contact plane having a vector connecting a representative pair of collision points detected by the use of a collision detecting algorithm as a normal vector is defined, a semi-contact polygon providing a contact shape with the contact plane every cube is acquired, and a common area of the semi-contact polygons of the cubes is calculated as a contact shape between the cubes. As shown in FIG. 9, in the example where two cubes collide with each other, a contact point indicated by C in the drawing is calculated.
A vertical repulsive force fN in the normal direction and frictional forces fFX and fFY perpendicular to each other in the direction perpendicular to the normal line act on the contact point C. In the dynamics simulation, the vertical repulsive force fN should be determined so that two cubes repel each other with a designated coefficient of restitution. Under an inequality constraint condition of “all the absolute values of the frictional forces fFX and fFY are equal to or smaller than a coefficient of friction μ×fN”, the frictional forces should be determined so that the relative velocity between two cubes is 0 (that is, so that the cubes stop by the balance of force).
When the value of the vertical repulsive force fN calculated by the dynamics simulation is too small, the cubes penetrate each other. On the contrary, when the value of the vertical repulsive force fN is too great, the cubes get apart from each other to cause the numerical divergence in some cases, thereby making the situation unstable. When the values of the frictional forces fFX and fFY calculated by the dynamics simulation are too small, one cube easily slides over the other cube and cannot stop. On the contrary, when the values of the frictional forces are too great, the numerical convergence is caused. In other words, it is necessary to calculate the vertical repulsive force fN and the frictional forces fFX and fFY with high precision in the dynamics simulation.
As a method of calculating the vertical repulsive force fN and the frictional forces fFX and fFY with high precision, a method called an “analysis type” is known. In the analysis type method, an external force allowing an object to reach a velocity after collision is determined in consideration of dynamics, and the calculation quantity becomes large but the external force can be calculated analytically with high accuracy.
Here, as an important concept for dynamics and motions, a concept called an “operational space” is known. The operational space is a space for describing relations between forces acting on objects and accelerations generated therefrom. The operations space is necessary when a way of contact between objects is used as constraints upon displaying a sense of force or controlling a force, not a position. The constraint condition can include, for example, a constraint on self interruption of a robot, a constraint on a joint movable angle, and an event that “a handle is fitted to an object” or “left and right eyes are horizontally balanced.”
In a link structure in which rigid links are linked through joints, a vector formed by connecting all the values of the joints to each other is called a generalization variable q. When the differential value of the generalization variable q with respect to time is connected with Expression (1) using Jacobian J, an operational space for a physical quantity x can be defined. The Cartesian coordinates system for carrying out tasks such as fingertip position and posture of a manipulator is an example of the operational space (For example, see “A unified approach to motion and force control of robot manipulators” (The operational space formulation, IEEE Journal of Robotics and Automation, RA-3(1), pp. 43-53, 1987)).{dot over (x)}=J{dot over (q)}  (1)
In general, it is known that the equation of motion of the entire link structure can be expressed by Expression (2).τ=H{umlaut over (q)}+b−JTf  (2)
In Expression (2), τ represents a generalization force corresponding to the generalization variable q, b represents the gravity and Coriolis force, f represents an external force acting on the operational space, and J represents Jacobian for correlating the operational space on which the external force f acts and the generalization variable q. By modifying Expression (2), the relation between the action force f and the acceleration (that is, two-step differential value of the physical quantity x with respect to time) generated in the action force direction (in the operational space) can be correlated with each other by the use of the equation of motion on the operational space expressed by Expression (3).{umlaut over (x)}=Λ−1f+c  (3)
In Expression (3), Λ−1 is called an operational space inertial inverse matrix and is expressed by Expression (4).Λ−1=JH−1JT  (4)
Here, H in Expression (4) represents an inertial matrix on the joint space of the entire structure. The relation between the acceleration and the force in the operational space is given by the operational space inertial inverse matrix Λ−1.
In addition, c of the third term in the right member of Expression (3) corresponds to an operational space biasing acceleration (that is, an acceleration generated in the operational space when no external force acts) and is expressed by Expression (5).c=JH−1(τ−b)+{dot over (J)}{dot over (q)}  (5)
When a physical quantity x is considered as a variable obtained by arranging the operational space as a whole, the problem of calculating a repulsive force can be described as a linear constraint condition including Expression (6) as linear equality and Expressions (7) and (8) as inequalities.{umlaut over (x)}=Λ−1f+c  (6)fNi≧0,{umlaut over (x)}Ni≧0 and fNi{umlaut over (x)}Ni=0  (7)|fFi|≦μifNi,fFi{umlaut over (x)}Fi≦0 and {umlaut over (x)}Fi(μifNi−|fFi|)=0  (8)
Expression (6) is an equation of motion (equal to Expression (3)) on the operational space in which the force f acting on the physical quantity x and the acceleration generated therefrom are correlated using the operational space inertial inverse matrix Λ−1. Expressions (7) and (8) are linear inequality constraints given to the action force f. In Expressions (7) and (8), fNi and fFi represent the external force and the frictional force in the normal direction at the action point (collision point) due to the constraint on contact with an external system. μi represents a friction coefficient at the action point. Expression (7) as the inequality constraint represents a condition where a repulsive force acts between the cubes in the vertical repulsive force direction of the collision point i between the contacting cubes so as not to penetrate each other. Expression (8) as the inequality constraint represents a friction condition.
The linear simultaneous equations including Expressions (6) to (8) are called a linear complementary problem (LCP). When the operational space inertial inverse matrix Λ−1 and the biasing acceleration c are known, the acceleration and the force f of x satisfying the conditions by solving the linear complementary problem. The mathematical solution of the linear complementary problem has been already suggested (for example, “Fast Contact Force Computation for Non-penetrating Rigid Bodies” (SIGGRAPH94, pp. 23-34, 1994)).
The operational space inertial inverse matrix Λ−1 is a square matrix having the total number of repulsive forces (in other words, the total number of collision points between cubes) as an order, but the calculation cost for the calculation is great. In order to calculate the acceleration and the force f of x by solving the linear complementary problem after acquiring the operational space inertial inverse matrix Λ−1, the same degree of calculation cost as the calculation of the operational space inertial inverse matrix Λ−1 is necessary. The calculation cost increases with the increase in the number of collision points between the cubes.
By solving the linear complementary problem using the analysis type technique (LCP solver), it is possible to strictly calculate the repulsive force acting between the cubes contacting or colliding with each other, thereby enhancing the stability in numerical calculation. However, there is a problem in that the simulation decreases in speed with the increase in the number of collision points between the cubes.
On the other hand, opposite to the analysis type technique, a “penalty method” of suppressing the calculation amount at the sacrifice of calculation precision is also known. In the penalty method, the repulsive force is calculated by virtually generating the force of spring and damper corresponding to the penetrating depth between the cubes contacting or colliding with each other. That is, as shown in FIG. 10, the restoring force and the damping force of the spring are calculated to correspond to the penetrating depth. In the penalty method, since it is not necessary to calculate the giant matrices such as the operational space inertial inverse matrix Λ−1 or to solve the linear complementary problem (Expressions (6) to (8)) after acquiring the operational space inertial inverse matrix Λ−1 and the biasing acceleration c, it is possible to solve the force of collision at a high speed.
However, when it is intended to simulate the collision between the rigid bodies by the use of the penalty method, it is necessary to insert very strong spring and damper in FIG. 10, thereby easily causing the numerical divergence. In order to avoid the numerical divergence, a countermeasure of shortening the period of integration can be considered. However, in this case, the calculation speed may be lowered in comparison with the analysis type method.
Accordingly, in the rigidity dynamics simulation, there is a need for an algorithm for calculating the contact force or the collision force acting between the rigid bodies contacting or colliding with each other with high precision and low calculation cost. The repulsive force acting between the objects can be calculated with high precision using the analysis type method, but the calculation of the operational space inertial inverse matrix Λ−1 or the calculation for solving the linear complementary problem serves as a bottle neck.